/*
 * @Author: gdtr
 * @Date: 2022-01-30 22:51:36
 * @LastEditors: OBKoro1
 * @LastEditTime: 2022-04-02 23:09:21
 * @FilePath: \myOdriveVSCode\USER\mylib\myFilter.cpp
 * @brief: 
 * 
 * Copyright (c) 2022 by 用户/公司名, All Rights Reserved. 
 */
//
// Created by 74354 on 2022/1/14.
//
#include "myFliter.hpp"
#include "math.h"



namespace myFilter
{

    void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
    {
        _cutoff_freq = cutoff_freq;
        if (_cutoff_freq <= 0.0f) {
            // no filtering
            return;
        }
        float fr = sample_freq/_cutoff_freq;
        float ohm = tanf(M_PI/fr);
        float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm;
        _b0 = ohm*ohm/c;
        _b1 = 2.0f*_b0;
        _b2 = _b0;
        _a1 = 2.0f*(ohm*ohm-1.0f)/c;
        _a2 = (1.0f-2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm)/c;
    }

    float LowPassFilter2p::apply(float sample)
    {
        if (_cutoff_freq <= 0.0f) {
            // no filtering
            return sample;
        }
        // do the filtering
        float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
        if (isnan(delay_element_0) || isinf(delay_element_0)) {
            // don't allow bad values to propogate via the filter
            delay_element_0 = sample;
        }
        float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;

        _delay_element_2 = _delay_element_1;
        _delay_element_1 = delay_element_0;
        // return the value.  Should be no need to check limits
        return output;
    }



} // namespace math

